
	#include "Imu9DOFDriver.h"

/******************************************************************************
*   Type Definitions
******************************************************************************/
	enum 
	{
		STATE0=0,
		STATE1,
		STATE2,
		STATE3,
		STATE4,
		STATE5,
		STATE6,
		STATE7,
		STATE8,
		MAXSTATE
	};
	
	enum 
	{
		STATE_A=0,	//read Accelerometer
		STATE_G ,	//Read Gyroscope
		STATE_M,	//Read MAgnometer
		MAXSTATE_IMU
	};
	

		//		EXTERNAL VARIABLES AS 9DOF OUTPUT			//
	///////////////////////////////////////////////////////////////
	volatile short XAxisAcceleration,YAxisAcceleration,ZAxisAcceleration;
	volatile short XAxisAngularRate,YAxisAngularRate,ZAxisAngularRate;
	volatile short XAxisMagnetic,YAxisMagnetic,ZAxisMagnetic;
	unsigned char  NewData; 
	
/******************************************************************************
*   					LOCAL VARIABLE DEFINITIONS
******************************************************************************/
	volatile unsigned char HighData,LowData;						//16bits Data receive from Sensors
	volatile unsigned char Sensor,RegisterAddress,DataToSend;			//8bits Data Send it to Sensor,Sensor and address sensor;
	volatile unsigned char i,NewDataFlag;			
	volatile unsigned char SampleCounter;
	unsigned short         WatchdogCounter;							//
	volatile signed short  AccelerometerSamples[(SAMPLE_NUMBER)*3];	//buffer containing 3 axis Accelerometer data
	volatile signed short  GyroscopeSamples[(SAMPLE_NUMBER)*3];		//Buffer containing 3 axis Gyroscope data
	volatile signed short  MagnetometerSamples[(SAMPLE_NUMBER)*3];	//Buffer containing 3 axis Magnetic data
	volatile signed short  *AccelerometerSamplesPtr,*GyroscopeSamplesPtr,*MagnetometerSamplesPtr;
	
	volatile short XAxisRawAcceleration,YAxisRawAcceleration,ZAxisRawAcceleration;
	volatile short XAxisRawAngularRate,YAxisRawAngularRate,ZAxisRawAngularRate;
	volatile short XAxisRawMagnetic,YAxisRawMagnetic,ZAxisRawMagnetic;
	
	SM SMSendData;
	SM SMReadData;
	SM SMReadIMU;
	
/******************************************************************************
*   					LOCAL FUNCTIONS PROTOTYPES
******************************************************************************/
	void VfnI2CSetup(unsigned char Frequency);
	void VfnIMUSetup(void);
	void VfnReadIMU(void);
	/*
	 * State machines needs to set the state to 0,
	 * Write to Sensor and RegisterAddress previous to call next functions
	 */
	void VfnSendDataI2C (void);		//State machine to Send 1 byte data
	void VfnReadDataI2C (void);		//State machine to receive 6 bytes data (3 - 2bytes)
	
	void (* const SendDataI2C[])(void);
	void (* const ReadDataI2C[])(void);
	
/******************************************************************************
*   					GLOBAL FUNCTIONS DEFINITIONS
******************************************************************************/

/*****************************************************************************
 * Function: 	Vfn9DOFDriverInitialization
 * Description: I2C Port Configuration and IMU 9DOF Configuration
 * Caveats:
 * Return:     
 *****************************************************************************/	
	void Vfn9DOFDriverInitialization (void)		
	{
		/* Initiate I2C port 
		 * Prepare State Machines /to send and receive data
		 * Initiate 9DOF sensor sitck
		 * Initaite Varaibles
		 * Get offset Values from 9DOF
		 */
		SMSendData.ActualState = STATE0;
		SMReadData.ActualState = STATE0;
		SMReadIMU.ActualState = STATE_A;
		
		VfnI2CSetup(I2C_400K);
		VfnIMUSetup();
		SampleCounter=SAMPLE_NUMBER;
		AccelerometerSamplesPtr=&AccelerometerSamples[0];
		GyroscopeSamplesPtr=&GyroscopeSamples[0];
		//VfnIMUSensorsOffset();
		unsigned char ii=0;
		while(ii!=50)						//Few "dummy" reads
		{
			Vfn9DOFDriver();
			if(NewData == 1)							
			{										
				NewData=0;
				ii++;
			}
		}
	}

/*****************************************************************************
 * Function: 	Vfn9DOFDriver
 * Description: Main loop to read the 9DOf sensor. Principal Fucntion *VfnReadIMU(),
 * 				main State machine to controll each state of read for each single sensor.
 * 				once all sensor are read. proceed to get sensors average.
 * Caveats:
 * Actualize:   X-Y-ZAxisAcceleration,X-Y-ZAxisAngularRate  & X-Y-ZAxisMagnetic
 *****************************************************************************/		
	void Vfn9DOFDriver (void)
	{
		volatile long Add1,Add2,Add3,Add4,Add5,Add6,Add7,Add8,Add9;
		Add1=0;Add2=0;Add3=0;Add4=0;Add5=0;Add6=0;Add7=0;Add8=0;Add9=0;
		VfnReadIMU();
		if(NewDataFlag == 1)									//if New data available	proceed	to storage.
		{
			NewDataFlag=0;	
			*AccelerometerSamplesPtr						= XAxisRawAcceleration;
			*((AccelerometerSamplesPtr)+(SAMPLE_NUMBER))	= YAxisRawAcceleration;
			*((AccelerometerSamplesPtr)+((SAMPLE_NUMBER)*2))= ZAxisRawAcceleration;
			*GyroscopeSamplesPtr							= XAxisRawAngularRate;
			*((GyroscopeSamplesPtr)+(SAMPLE_NUMBER))		= YAxisRawAngularRate;
			*((GyroscopeSamplesPtr)+((SAMPLE_NUMBER)*2))	= ZAxisRawAngularRate;
			*MagnetometerSamplesPtr							= XAxisRawMagnetic;
			*((MagnetometerSamplesPtr)+(SAMPLE_NUMBER))		= YAxisRawMagnetic;
			*((MagnetometerSamplesPtr)+((SAMPLE_NUMBER)*2))	= ZAxisRawMagnetic;
			SampleCounter--;
			if(SampleCounter==0)							//untill the SampleCounter is equal to SAMPLE_NUMBER "just the first time"
			{												//"to fill the all buffer"
				i=0;
				AccelerometerSamplesPtr = &AccelerometerSamples[0];
				GyroscopeSamplesPtr     = &GyroscopeSamples[0];
				MagnetometerSamplesPtr  = &MagnetometerSamples[0];
				while(i!=SAMPLE_NUMBER)						//once the buffers are full, proceed to add all samples from each buffer
	 			{
	 				Add1 +=   *AccelerometerSamplesPtr;
	 				Add2 += *((AccelerometerSamplesPtr)+(SAMPLE_NUMBER));
	 				Add3 += *((AccelerometerSamplesPtr)+((SAMPLE_NUMBER)*2));
	 				Add4 +=   *GyroscopeSamplesPtr;
	 				Add5 += *((GyroscopeSamplesPtr)+(SAMPLE_NUMBER));
	 				Add6 += *((GyroscopeSamplesPtr)+((SAMPLE_NUMBER)*2));
	 				Add7 +=   *MagnetometerSamplesPtr;
	 				Add8 += *((MagnetometerSamplesPtr)+(SAMPLE_NUMBER));
	 				Add9 += *((MagnetometerSamplesPtr)+((SAMPLE_NUMBER)*2));
	 				AccelerometerSamplesPtr++;
					GyroscopeSamplesPtr++;
					MagnetometerSamplesPtr++;
	 				i++;
	 			}
	 			XAxisAcceleration = (Add1>>DIVISOR);		//obtainig the average from each axis
	 			YAxisAcceleration = (Add2>>DIVISOR);	 	//		"Accelerometer"
	 			ZAxisAcceleration = (Add3>>DIVISOR);
	 			XAxisAngularRate  = (Add4>>DIVISOR);			//		"Gyroscope"	
	 			YAxisAngularRate  = (Add5>>DIVISOR);
	 			ZAxisAngularRate  = (Add6>>DIVISOR);
	 			XAxisMagnetic     = (Add7>>DIVISOR);
	 			YAxisMagnetic 	  = (Add8>>DIVISOR);
	 			ZAxisMagnetic  	  = (Add9>>DIVISOR);
	 			Add1=0;Add2=0;Add3=0;Add4=0;Add5=0;Add6=0;Add7=0;Add8=0;Add9=0;
	 			i=0;
	 			while(i!=((SAMPLE_NUMBER)-1))						//Sample Shifter "Remove the oldest sample"
	   			{
	   				AccelerometerSamples[i]						  = AccelerometerSamples[i+1];
	   				AccelerometerSamples[(i)+(SAMPLE_NUMBER)] 	  = AccelerometerSamples[((i)+(SAMPLE_NUMBER))+1];
	   				AccelerometerSamples[(i)+((SAMPLE_NUMBER)*2)] = AccelerometerSamples[((i)+((SAMPLE_NUMBER)*2))+1];
	   				GyroscopeSamples[i]							  = GyroscopeSamples[i+1];
	   				GyroscopeSamples[(i)+(SAMPLE_NUMBER)]		  = GyroscopeSamples[((i)+(SAMPLE_NUMBER))+1];
	   				GyroscopeSamples[(i)+((SAMPLE_NUMBER)*2)]	  = GyroscopeSamples[((i)+((SAMPLE_NUMBER)*2))+1];
	   				MagnetometerSamples[i]						  = MagnetometerSamples[i+1];
	   				MagnetometerSamples[(i)+(SAMPLE_NUMBER)]	  = MagnetometerSamples[((i)+(SAMPLE_NUMBER))+1];
	   				MagnetometerSamples[(i)+((SAMPLE_NUMBER)*2)]  = MagnetometerSamples[((i)+((SAMPLE_NUMBER)*2))+1];
	   				i++;
	   			}
	 			
				SampleCounter=1;														//SampleCounter = 1 cause from this point
				AccelerometerSamplesPtr = &AccelerometerSamples[(SAMPLE_NUMBER)-1];		//"the buffer" is almost full, and needs 
				GyroscopeSamplesPtr     = &GyroscopeSamples[(SAMPLE_NUMBER)-1];			//to be fill it up, with recent data.
				MagnetometerSamplesPtr  = &MagnetometerSamples[(SAMPLE_NUMBER)-1];
			}
			else{													//Buffer still filling
				AccelerometerSamplesPtr++;
				GyroscopeSamplesPtr++;
				MagnetometerSamplesPtr++;
			}
		}
	}
	
	void VfnReset9DOFDriver (void)
	{
		/* Disable and Re-enable I2C port 
		 * Prepare State Machines /to send and receive data
		 * Initiate 9DOF sensor sitck
		 * Initaite Varaible
		 */
		#if I2C_MODULE == I2C0
		I2CMasterDisable(I2C0_MASTER_BASE);
		SysCtlPeripheralDisable(SYSCTL_PERIPH_I2C0);					//Clock to I2C0 module
		SysCtlPeripheralDisable(SYSCTL_PERIPH_GPIOB);
		#endif
		
		#if I2C_MODULE == I2C1
		I2CMasterDisable(I2C1_MASTER_BASE);
		SysCtlPeripheralDisable(SYSCTL_PERIPH_I2C1);					//Clock to I2C0 module
		SysCtlPeripheralDisable(SYSCTL_PERIPH_GPIOB);
		#endif
	
		SMSendData.ActualState = STATE0;
		SMReadData.ActualState = STATE0;
		SMReadIMU.ActualState = STATE_A;
		
		VfnI2CSetup(I2C_400K);
		VfnIMUSetup();
		SampleCounter=SAMPLE_NUMBER;
		AccelerometerSamplesPtr=&AccelerometerSamples[0];
		GyroscopeSamplesPtr=&GyroscopeSamples[0];	
	}
	void VfnWatchDog9DOFDriver (void)
	{
		if(WatchdogCounter == 50 && XAxisAcceleration == 0 && XAxisAngularRate == 0)
		{
			WatchdogCounter = 0;
			VfnReset9DOFDriver();
		}
//		if()
//		{
//			WatchdogCounter++;
//		}
	}


/******************************************************************************
*   					LOCAL FUNCTIONS DEFINITIONS
******************************************************************************/

/*****************************************************************************
 * Function: 	VfnI2CSetup
 * Description: I2C Module Configuration, at a specific frequency.
 * Parameters:	Frequency
 * Caveats:		I2C Module to be setup depends on the Macro Definitions
 * Return:		N/A
 *****************************************************************************/	
	void VfnI2CSetup(unsigned char Frequency)
	{
		//////////////////////////////////////////////
		//		I2C MODULE CONFIGURATION			//
		//////////////////////////////////////////////
		#if (I2C_MODULE == I2C0)
		SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C0);					//Clock to I2C0 module
		SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);				//clock to PORTB module
		GPIOPinTypeI2C( GPIO_PORTB_BASE, GPIO_PIN_2 | GPIO_PIN_3 );	//I2C's pins configuration
		if(Frequency==4)
		{
    		I2CMasterInitExpClk(I2C0_MASTER_BASE, SysCtlClockGet(), true);	//I2C frequency at 400khz
		}
		if(Frequency==1)
		{
			I2CMasterInitExpClk(I2C0_MASTER_BASE, SysCtlClockGet(), false);	//I2C frequency at 100khz
		}	
    	I2CMasterEnable( I2C0_MASTER_BASE );
		#endif
		#if (I2C_MODULE == I2C1)
		SysCtlPeripheralEnable(SYSCTL_PERIPH_I2C1);					//Clock to I2C1 module
		SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);				//clock to PORTA module
		GPIOPinTypeI2C( GPIO_PORTA_BASE, GPIO_PIN_6 | GPIO_PIN_7 );	//I2C's pins configurations
		if(Frequency==4)
		{
    		I2CMasterInitExpClk(I2C1_MASTER_BASE, SysCtlClockGet(), true);	//I2C frequency at 400khz
		}
		if(Frequency==1)
		{
			I2CMasterInitExpClk(I2C1_MASTER_BASE, SysCtlClockGet(), false);	//I2C frequency at 100khz
		}	
    	I2CMasterEnable( I2C1_MASTER_BASE );
		#endif	
	}

/*****************************************************************************
 * Function: 	VfnIMUSetup
 * Description: 9DOF Sensors Configuration, Power-Mode, Scale, Gain and Freq.
 * Parameters:	N/A
 * Caveats:		Scale, Gains and Freq. Sample are attach to this Function. 
 * 				*Note: Scale, Gain and Freq. Sample must be modified in this Function
 * Return:		N/A
 *****************************************************************************/		
	void VfnIMUSetup( void )
	{
		//////////////////////////////////////////
    	//			ACCELEROMTER POWER-UP		//
    	//////////////////////////////////////////
    	Sensor = ACCELEROMETER_ADDRESS;
    	RegisterAddress = POWER_CTL_R;				
    	DataToSend = 0x00;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
   		Sensor = ACCELEROMETER_ADDRESS;
    	RegisterAddress = POWER_CTL_R;
    	DataToSend = MEASURE_MODE;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
		
		//////////////////////////////////////////
    	//			GYROSCOPE POWER-UP			//
    	//////////////////////////////////////////
    	Sensor = GYROSCOPE_ADDRESS;
    	RegisterAddress = PWR_MGM_R;
    	DataToSend = 0x00;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	Sensor = GYROSCOPE_ADDRESS;
    	RegisterAddress = PWR_MGM_R;
    	DataToSend = GYRO_RESET;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	
		//////////////////////////////////////////
    	//			ACCELEROMETER SETUP			//
    	//////////////////////////////////////////
    	Sensor = ACCELEROMETER_ADDRESS;
    	RegisterAddress = BW_RATE_R;
    	DataToSend = ACCELEROMETER_OUTPUT_RATE;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	Sensor = ACCELEROMETER_ADDRESS;
    	RegisterAddress = DATA_FORMAT_R;
    	DataToSend = ACCELEROMETER_DATA_FORMAT;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
		
		//////////////////////////////////////////
    	//			GYROSCOPE SETUP				//
    	//////////////////////////////////////////
    	Sensor = GYROSCOPE_ADDRESS;
    	RegisterAddress = SMPLTR_DIV_R;
    	DataToSend = GYROSCOPE_FREQUENCY_DIVIDER;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	Sensor = GYROSCOPE_ADDRESS;
    	RegisterAddress = DLPFS_FS_R;
    	DataToSend = GYROSCOPE_RATE_AND_SCALE ;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	Sensor = GYROSCOPE_ADDRESS;
    	RegisterAddress = INT_CFG_R;					//Default: No pin interrupts activated
    	DataToSend = 0x00;							
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	
    	//////////////////////////////////////////
    	//			MAGNETOMATER SETUP			//
    	//////////////////////////////////////////
    	Sensor          = MAGNETOMETER_ADDRESS;
    	RegisterAddress = CONFIG_REG_A;
    	DataToSend      = MAGNETOMETER_OUTPUT_RATE;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	Sensor          = MAGNETOMETER_ADDRESS;
    	RegisterAddress = CONFIG_REG_B;
    	DataToSend      = MAGNETOMETER_GAIN;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	Sensor          = MAGNETOMETER_ADDRESS;
    	RegisterAddress = MODE_REGISTER;
    	DataToSend      = OPERATION_MODE;
    	SMSendData.ActualState = STATE0;
    	while(SMSendData.ActualState != MAXSTATE)
    	{
   			SendDataI2C[SMSendData.ActualState]();
    	}
    	
   
	}
	
/*****************************************************************************
 * Function: 	VfnReadIMU
 * Description: This Function read the IMU's sensors information 
 * 				using a State MAchin for each sesnor. 
 * 				The Accelerometer is the first sensor to read, the Gyroscoper
 * 				and for the last the magnetometer
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnReadIMU(void)	
	{
		if(SMReadIMU.ActualState == STATE_A)
		{
			Sensor = ACCELEROMETER_ADDRESS;
    		RegisterAddress = 0x32;
			ReadDataI2C[SMReadData.ActualState]();
			if(SMReadData.ActualState == MAXSTATE)
			{
				SMReadData.ActualState = STATE0;
				SMReadIMU.ActualState = STATE_G;
			}
		}
		if(SMReadIMU.ActualState == STATE_G)
		{
			Sensor = GYROSCOPE_ADDRESS;
    		RegisterAddress = 0x1D;
			ReadDataI2C[SMReadData.ActualState]();
			if(SMReadData.ActualState == MAXSTATE)
			{
				SMReadData.ActualState = STATE0;
				SMReadIMU.ActualState = STATE_M;
			}
		}
		if(SMReadIMU.ActualState == STATE_M)
		{
			Sensor = MAGNETOMETER_ADDRESS;
    		RegisterAddress = 0x03;
			ReadDataI2C[SMReadData.ActualState]();
			if(SMReadData.ActualState == MAXSTATE)
			{
				SMReadData.ActualState = STATE0;
				SMReadIMU.ActualState = STATE_A;
				NewData=1;
				NewDataFlag=1;
			}
		}
	}


	//////////////////////////////////////////////////////////////
	//				STATE MACHINE TO SEND DATA I2C				//
	//////////////////////////////////////////////////////////////
/*****************************************************************************
 * Function: 	VfnSendDataState0
 * Description: Write Slave address and a single data (command) to send
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnSendDataState0 (void)
	{
		#if (I2C_MODULE == I2C0)
		I2CMasterSlaveAddrSet(I2C0_MASTER_BASE, Sensor, false);
    	I2CMasterDataPut(I2C0_MASTER_BASE, RegisterAddress	 );							
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_SEND_START);
		#endif
		#if (I2C_MODULE == I2C1)
		I2CMasterSlaveAddrSet(I2C1_MASTER_BASE, Sensor, false);
    	I2CMasterDataPut(I2C1_MASTER_BASE, RegisterAddress	 );							
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_BURST_SEND_START);
		#endif
		SMSendData.ActualState = STATE3;
		SMSendData.NextState   = STATE1;
	}
	
/*****************************************************************************
 * Function: 	VfnSendDataState1
 * Description: Command to I2C module to send Direcion and data.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnSendDataState1 (void)
	{
		#if (I2C_MODULE == I2C0)
		I2CMasterDataPut(I2C0_MASTER_BASE, DataToSend);
		#endif
		#if (I2C_MODULE == I2C1)
		I2CMasterDataPut(I2C1_MASTER_BASE, DataToSend);
		#endif
		SMSendData.ActualState = STATE3;
		SMSendData.NextState   = STATE2;
	}
	
/*****************************************************************************
 * Function: 	VfnSendDataState2
 * Description: Command to I2C module to finish transmission.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnSendDataState2 (void)
	{
		#if (I2C_MODULE == I2C0)
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
		#endif
		#if (I2C_MODULE == I2C1)
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_BURST_SEND_FINISH);
		#endif
		SMSendData.ActualState = STATE3;
		SMSendData.NextState   = MAXSTATE;
	}
	
/*****************************************************************************
 * Function: 	VfnSendDataState3
 * Description: Check if I2C module still busy, if not, jump next state.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnSendDataState3 (void)
	{
		#if (I2C_MODULE == I2C0)
		if(!(I2CMasterBusy(I2C0_MASTER_BASE)))
		{
			SMSendData.ActualState = SMSendData.NextState;
		}
		#endif
		#if (I2C_MODULE == I2C1)
		if(!(I2CMasterBusy(I2C1_MASTER_BASE)))
		{
			SMSendData.ActualState = SMSendData.NextState;
		}
		#endif
	}

/*****************************************************************************
 * Function: 	void (* const SendDataI2C[])(void)
 * Description: Functions array. representing each state.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/		
	void (* const SendDataI2C[])(void)	=
	{
		VfnSendDataState0,
		VfnSendDataState1,
		VfnSendDataState2,
		VfnSendDataState3,
	};
	
	
	//////////////////////////////////////////////////////////////
	//				STATE MACHINE TO READ DATA I2C				//
	//////////////////////////////////////////////////////////////
/*****************************************************************************
 * Function: 	VfnReadDataState0
 * Description: Write Address and Data to be sent to Slave.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnReadDataState0 (void)
	{
		#if (I2C_MODULE == I2C0)
		I2CMasterSlaveAddrSet(I2C0_MASTER_BASE, Sensor, false);
   		I2CMasterDataPut(I2C0_MASTER_BASE, RegisterAddress);
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_SINGLE_SEND );
		#endif
		#if (I2C_MODULE == I2C1)
		I2CMasterSlaveAddrSet(I2C1_MASTER_BASE, Sensor, false);
   		I2CMasterDataPut(I2C1_MASTER_BASE, RegisterAddress);
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_SINGLE_SEND );
		#endif
		SMReadData.ActualState = STATE8;
		SMReadData.NextState   = STATE1;
	}
/*****************************************************************************
 * Function: 	VfnReadDataState1
 * Description: Sends a ReStart to Slave and read command "included".
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnReadDataState1 (void)
	{
		#if (I2C_MODULE == I2C0)
		I2CMasterSlaveAddrSet(I2C0_MASTER_BASE,Sensor, true);
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_START );
		#endif
		#if (I2C_MODULE == I2C1)
		I2CMasterSlaveAddrSet(I2C1_MASTER_BASE,Sensor, true);
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_START );
		#endif
		SMReadData.ActualState = STATE8;
		SMReadData.NextState   = STATE2;
	}

/*****************************************************************************
 * Function: 	VfnReadDataState2
 * Description: Get firt Data byte  "LowData" From the sensor.
 * 				- X Axis.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnReadDataState2 (void)
	{
		#if (I2C_MODULE == I2C0)
		LowData= I2CMasterDataGet(I2C0_MASTER_BASE);
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT );
		#endif
		#if (I2C_MODULE == I2C1)
		LowData= I2CMasterDataGet(I2C1_MASTER_BASE);
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT );
		#endif
		SMReadData.ActualState = STATE8;
		SMReadData.NextState   = STATE3;
	}

/*****************************************************************************
 * Function: 	VfnReadDataState3
 * Description: Get Second data byte "HighData" from the sensor then, Put High and Low data
 * 				on a single variable.
 * 				- X Axis.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnReadDataState3 (void)
	{
		#if (I2C_MODULE == I2C0)
		HighData= (I2CMasterDataGet(I2C0_MASTER_BASE));
		#endif
		#if (I2C_MODULE == I2C1)
		HighData= (I2CMasterDataGet(I2C1_MASTER_BASE));
		#endif
			if(Sensor==ACCELEROMETER_ADDRESS)
			{
				XAxisRawAcceleration=0;
				XAxisRawAcceleration=HighData<<8;
				XAxisRawAcceleration+=LowData;
			}
			if(Sensor==GYROSCOPE_ADDRESS)
			{
				XAxisRawAngularRate=0;
				XAxisRawAngularRate=LowData<<8;
				XAxisRawAngularRate+=HighData;
			}
			if(Sensor==MAGNETOMETER_ADDRESS)
			{
				XAxisRawMagnetic=0;
				XAxisRawMagnetic=LowData<<8;
				XAxisRawMagnetic+=HighData;
			}
		#if (I2C_MODULE == I2C0)
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT );
		#endif
		#if (I2C_MODULE == I2C1)
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT );
		#endif
		SMReadData.ActualState = STATE8;
		SMReadData.NextState   = STATE4;
	}

/*****************************************************************************
 * Function: 	VfnReadDataState4
 * Description: Get firt Data byte  "LowData" From the sensor.
 * 				- Y Axis.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/
	void VfnReadDataState4 (void)
	{
		#if (I2C_MODULE == I2C0)
		LowData= (I2CMasterDataGet(I2C0_MASTER_BASE));
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT );
		#endif
		#if (I2C_MODULE == I2C1)
		LowData= (I2CMasterDataGet(I2C1_MASTER_BASE));
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT );
		#endif
		SMReadData.ActualState = STATE8;
		SMReadData.NextState   = STATE5;
	}
	
/*****************************************************************************
 * Function: 	VfnReadDataState5
 * Description: Get Second data byte "HighData" from the sensor then, Put High and Low data
 * 				on a single variable.
 * 				- Y Axis.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnReadDataState5 (void)
	{
		#if (I2C_MODULE == I2C0)
		HighData= (I2CMasterDataGet(I2C0_MASTER_BASE));
		#endif
		#if (I2C_MODULE == I2C1)
		HighData= (I2CMasterDataGet(I2C1_MASTER_BASE));
		#endif
			if(Sensor==ACCELEROMETER_ADDRESS)
			{
				YAxisRawAcceleration=0;
				YAxisRawAcceleration=HighData<<8;
				YAxisRawAcceleration+=LowData;
			}
			if(Sensor==GYROSCOPE_ADDRESS)
			{
				YAxisRawAngularRate=0;
				YAxisRawAngularRate=LowData<<8;
				YAxisRawAngularRate+=HighData;
			}
			if(Sensor==MAGNETOMETER_ADDRESS)
			{
				ZAxisRawMagnetic=0;
				ZAxisRawMagnetic=LowData<<8;
				ZAxisRawMagnetic+=HighData;
			}
		#if (I2C_MODULE == I2C0)
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT );
		#endif
		#if (I2C_MODULE == I2C1)
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_CONT );
		#endif
		SMReadData.ActualState = STATE8;
		SMReadData.NextState   = STATE6;
	}
	
/*****************************************************************************
 * Function: 	VfnReadDataState6
 * Description: Get firt Data byte  "LowData" From the sensor.
 * 				- Z Axis.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/
	void VfnReadDataState6 (void)
	{
		#if (I2C_MODULE == I2C0)
		LowData= (I2CMasterDataGet(I2C0_MASTER_BASE));
		I2CMasterControl(I2C0_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_FINISH  );
		#endif
		#if (I2C_MODULE == I2C1)
		LowData= (I2CMasterDataGet(I2C1_MASTER_BASE));
		I2CMasterControl(I2C1_MASTER_BASE, I2C_MASTER_CMD_BURST_RECEIVE_FINISH  );
		#endif
		SMReadData.ActualState = STATE8;
		SMReadData.NextState   = STATE7;
	}

/*****************************************************************************
 * Function: 	VfnReadDataState7
 * Description: Get Second data byte "HighData" from the sensor then, Put High and Low data
 * 				on a single variable.
 * 				- Z Axis.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnReadDataState7 (void)
	{
		#if (I2C_MODULE == I2C0)
		HighData= (I2CMasterDataGet(I2C0_MASTER_BASE));
		#endif
		#if (I2C_MODULE == I2C1)
		HighData= (I2CMasterDataGet(I2C1_MASTER_BASE));
		#endif
			if(Sensor==ACCELEROMETER_ADDRESS)
			{
				ZAxisRawAcceleration  = 0;
				ZAxisRawAcceleration  = HighData<<8;
				ZAxisRawAcceleration += LowData;
			}
			if(Sensor==GYROSCOPE_ADDRESS)
			{
				ZAxisRawAngularRate  = 0;
				ZAxisRawAngularRate  = LowData<<8;
				ZAxisRawAngularRate += HighData;
			}
			if(Sensor==MAGNETOMETER_ADDRESS)
			{
				YAxisRawMagnetic=0;
				YAxisRawMagnetic=LowData<<8;
				YAxisRawMagnetic+=HighData;
			}
		SMReadData.ActualState = STATE8;
		SMReadData.NextState   = MAXSTATE;
	}

/*****************************************************************************
 * Function: 	VfnReadDataState8
 * Description: Check if I2C module still busy, if not, jump next state.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/	
	void VfnReadDataState8 (void)
	{
		#if (I2C_MODULE == I2C0)
		if(!(I2CMasterBusy(I2C0_MASTER_BASE)))
		{
			SMReadData.ActualState = SMReadData.NextState;
		}
		#endif
		#if (I2C_MODULE == I2C1)
		if(!(I2CMasterBusy(I2C1_MASTER_BASE)))
		{
			SMReadData.ActualState = SMReadData.NextState;
		}
		#endif
	}

/*****************************************************************************
 * Function: 	void (* const ReadDataI2C[])(void)
 * Description: Function Array. Representing each state.
 * Parameters:	N/A
 * Caveats:		Non
 * Return:		N/A
 *****************************************************************************/		
	void (* const ReadDataI2C[])(void)	=
	{
		VfnReadDataState0,
		VfnReadDataState1,
		VfnReadDataState2,
		VfnReadDataState3,
		VfnReadDataState4,
		VfnReadDataState5,
		VfnReadDataState6,
		VfnReadDataState7,
		VfnReadDataState8,
	};
